#include "remote_control.h"
#include "main.h"
#include "bsp_usart.h"
#include "string.h"
#include "detect_task.h"
#include "debug_config.h"

// 遥控器出错数据上限
#define RC_CHANNAL_ERROR_VALUE 900

extern UART_HandleTypeDef huart3;
extern DMA_HandleTypeDef hdma_usart3_rx;
static uint8_t rcPowerlessFlag=1;                   // 1表示遥控器掉线，0表示上线（天地飞遥控器掉线接收机发消息）

// 取正函数
static int16_t RC_abs(int16_t value);
/**
 * @brief          remote control protocol resolution
 * @param[in]      sbus_buf: raw data point
 * @param[out]     rc_ctrl: remote control data struct point
 * @retval         none
 */
/**
 * @brief          遥控器协议解析
 * @param[in]      sbus_buf: 原生数据指针
 * @param[out]     rc_ctrl: 遥控器数据指
 * @retval         none
 */
static void sbus_to_rc(volatile const uint8_t *sbus_buf, RC_ctrl_t *rc_ctrl);

// remote control data
// 遥控器控制变量
RC_ctrl_t rc_ctrl;
// 接收原始数据，为25个字节，给了50个字节长度，防止DMA传输越界
static uint8_t sbus_rx_buf[2][SBUS_RX_BUF_NUM];
#ifdef STM_STUDIO_RC_BUF
uint8_t sbusBuf0,sbusBuf1,sbusBuf2,sbusBuf3,sbusBuf4,sbusBuf5,sbusBuf6,sbusBuf7,sbusBuf8,sbusBuf9,sbusBuf10,sbusBuf11,sbusBuf12,sbusBuf13,sbusBuf14,sbusBuf15,sbusBuf16,sbusBuf17,sbusBuf18,sbusBuf19,sbusBuf20,sbusBuf21,sbusBuf22,sbusBuf23,sbusBuf24;

void sbus_rx_buf_to_stm_studio(uint8_t i){
    sbusBuf0=sbus_rx_buf[i][0];
    sbusBuf1=sbus_rx_buf[i][1];
    sbusBuf2=sbus_rx_buf[i][2];
    sbusBuf3=sbus_rx_buf[i][3];
    sbusBuf4=sbus_rx_buf[i][4];
    sbusBuf5=sbus_rx_buf[i][5];
    sbusBuf6=sbus_rx_buf[i][6];
    sbusBuf7=sbus_rx_buf[i][7];
    sbusBuf8=sbus_rx_buf[i][8];
    sbusBuf9=sbus_rx_buf[i][9];
    sbusBuf10=sbus_rx_buf[i][10];
    sbusBuf11=sbus_rx_buf[i][11];
    sbusBuf12=sbus_rx_buf[i][12];
    sbusBuf13=sbus_rx_buf[i][13];
    sbusBuf14=sbus_rx_buf[i][14];
    sbusBuf15=sbus_rx_buf[i][15];
    sbusBuf16=sbus_rx_buf[i][16];
    sbusBuf17=sbus_rx_buf[i][17];
    sbusBuf18=sbus_rx_buf[i][18];
    sbusBuf19=sbus_rx_buf[i][19];
    sbusBuf20=sbus_rx_buf[i][20];
    sbusBuf21=sbus_rx_buf[i][21];
    sbusBuf22=sbus_rx_buf[i][22];
    sbusBuf23=sbus_rx_buf[i][23];
    sbusBuf24=sbus_rx_buf[i][24];
}
#endif

#ifdef SIM_STUDIO_RC_CH_CHANNEL
int16_t rcCh0,rcCh1,rcCh2,rcCh3,rcCh4,rcCh5,rcCh6,rcCh7;
#define RCCH(i) rcCh ## i = rc_ctrl.rc.ch[i]
void rcCh_to_stm_studio(void){
    RCCH(0);
    RCCH(1);
    RCCH(2);
    RCCH(3);
    RCCH(4);
    RCCH(5);
    RCCH(6);
    RCCH(7);
}
#endif

/**
 * @brief          remote control init
 * @param[in]      none
 * @retval         none
 */
/**
 * @brief          遥控器初始化
 * @param[in]      none
 * @retval         none
 */
void remote_control_init(void)
{
    RC_Init(sbus_rx_buf[0], sbus_rx_buf[1], SBUS_RX_BUF_NUM);
}
/**
 * @brief          get remote control data point
 * @param[in]      none
 * @retval         remote control data point
 */
/**
 * @brief          获取遥控器数据指针
 * @param[in]      none
 * @retval         遥控器数据指针
 */
const RC_ctrl_t *get_remote_control_point(void)
{
    return &rc_ctrl;
}

// 判断遥控器数据是否出错，
uint8_t RC_data_is_error(void)
{
    // 使用了go to语句 方便出错统一处理遥控器变量数据归零
    if (RC_abs(rc_ctrl.rc.ch[0]) > RC_CHANNAL_ERROR_VALUE)
    {
        goto error;
    }
    if (RC_abs(rc_ctrl.rc.ch[1]) > RC_CHANNAL_ERROR_VALUE)
    {
        goto error;
    }
    if (RC_abs(rc_ctrl.rc.ch[2]) > RC_CHANNAL_ERROR_VALUE)
    {
        goto error;
    }
    if (RC_abs(rc_ctrl.rc.ch[3]) > RC_CHANNAL_ERROR_VALUE)
    {
        goto error;
    }
    if (rc_ctrl.rc.ch[6] == 0)
    {
        goto error;
    }
    if (rc_ctrl.rc.ch[7] == 0)
    {
        goto error;
    }
    return 0;

error: // 有修改，需要验证
    rc_ctrl.rc.ch[0] = 0;
    rc_ctrl.rc.ch[1] = 0;
    rc_ctrl.rc.ch[2] = 0;
    rc_ctrl.rc.ch[3] = 0;
    rc_ctrl.rc.ch[4] = 0;
    rc_ctrl.rc.ch[5] = 0;
    rc_ctrl.rc.ch[6] = 0;
    rc_ctrl.rc.ch[7] = 0;
    return 1;
}

void slove_RC_lost(void)
{
    RC_restart(SBUS_RX_BUF_NUM);
}
void slove_data_error(void)
{
    RC_restart(SBUS_RX_BUF_NUM);
}

// 串口中断
void USART3_IRQHandler(void)
{
    if (huart3.Instance->SR & UART_FLAG_RXNE) // 接收到数据
    {
        __HAL_UART_CLEAR_PEFLAG(&huart3);
    }
    else if (USART3->SR & UART_FLAG_IDLE)
    {
        static uint16_t this_time_rx_len = 0;

        __HAL_UART_CLEAR_PEFLAG(&huart3);

        if ((hdma_usart3_rx.Instance->CR & DMA_SxCR_CT) == RESET)
        {
            /* Current memory buffer used is Memory 0 */

            // disable DMA
            // 失效DMA
            __HAL_DMA_DISABLE(&hdma_usart3_rx);

            // get receive data length, length = set_data_length - remain_length
            // 获取接收数据长度,长度 = 设定长度 - 剩余长度
            this_time_rx_len = SBUS_RX_BUF_NUM - hdma_usart3_rx.Instance->NDTR;

            // reset set_data_lenght
            // 重新设定数据长度
            hdma_usart3_rx.Instance->NDTR = SBUS_RX_BUF_NUM;

            // set memory buffer 1
            // 设定缓冲区1
            hdma_usart3_rx.Instance->CR |= DMA_SxCR_CT;

            // enable DMA
            // 使能DMA
            __HAL_DMA_ENABLE(&hdma_usart3_rx);

            if (this_time_rx_len == RC_FRAME_LENGTH)
            {
                sbus_to_rc(sbus_rx_buf[0], &rc_ctrl);
                // 记录数据接收时间
                detect_hook(DBUS_TOE);
                #ifdef STM_STUDIO_RC_BUF
                sbus_rx_buf_to_stm_studio(0);
                #endif
            }
        }
        else
        {
            /* Current memory buffer used is Memory 1 */
            // disable DMA
            // 失效DMA
            __HAL_DMA_DISABLE(&hdma_usart3_rx);

            // get receive data length, length = set_data_length - remain_length
            // 获取接收数据长度,长度 = 设定长度 - 剩余长度
            this_time_rx_len = SBUS_RX_BUF_NUM - hdma_usart3_rx.Instance->NDTR;

            // reset set_data_lenght
            // 重新设定数据长度
            hdma_usart3_rx.Instance->NDTR = SBUS_RX_BUF_NUM;

            // set memory buffer 0
            // 设定缓冲区0
            DMA1_Stream1->CR &= ~(DMA_SxCR_CT);

            // enable DMA
            // 使能DMA
            __HAL_DMA_ENABLE(&hdma_usart3_rx);

            if (this_time_rx_len == RC_FRAME_LENGTH)
            {
                // 处理遥控器数据
                sbus_to_rc(sbus_rx_buf[1], &rc_ctrl);
                // 记录数据接收时间
                detect_hook(DBUS_TOE);
                #ifdef STM_STUDIO_RC_BUF
                sbus_rx_buf_to_stm_studio(1);
                #endif
            }
        }
    }
}

// 取正函数
static int16_t RC_abs(int16_t value)
{
    if (value > 0)
    {
        return value;
    }
    else
    {
        return -value;
    }
}
/**
 * @brief          remote control protocol resolution
 * @param[in]      sbus_buf: raw data point
 * @param[out]     rc_ctrl: remote control data struct point
 * @retval         none
 */
/**
 * @brief          遥控器协议解析
 * @param[in]      sbus_buf: 原生数据指针
 * @param[out]     rc_ctrl: 遥控器数据指
 * @retval         none
 */
static void sbus_to_rc(volatile const uint8_t *sbus_buf, RC_ctrl_t *rc_ctrl)
{
    if (sbus_buf == NULL || rc_ctrl == NULL)
    {
        return;
    }
    if(sbus_buf[0]==15&&sbus_buf[1]==0&&sbus_buf[2]==4&&sbus_buf[3]==96&&sbus_buf[4]==0&&sbus_buf[5]==0&&sbus_buf[6]==8&&sbus_buf[7]==64&&sbus_buf[8]==0&&sbus_buf[9]==2&&sbus_buf[10]==16&&sbus_buf[11]==128&&sbus_buf[12]==0&&sbus_buf[13]==4&&sbus_buf[14]==32&&sbus_buf[15]==0&&sbus_buf[16]==1&&sbus_buf[17]==8&&sbus_buf[18]==64&&sbus_buf[19]==0&&sbus_buf[20]==2&&sbus_buf[21]==16&&sbus_buf[22]==128&&sbus_buf[23]==12&&sbus_buf[24]==0){
        rcPowerlessFlag=1;
    }else{
        if (sbus_buf[0] == 0x0f)
        {
            rcPowerlessFlag=0;
            rc_ctrl->rc.ch[0] = ((sbus_buf[1] >> 0 | sbus_buf[2] << 8) & 0x07FF);                     // Channel 1
            rc_ctrl->rc.ch[1] = ((sbus_buf[2] >> 3 | sbus_buf[3] << 5) & 0x07FF);                     // Channel 2
            rc_ctrl->rc.ch[2] = ((sbus_buf[3] >> 6 | sbus_buf[4] << 2 | sbus_buf[5] << 10) & 0x07FF); // Channel 3
            rc_ctrl->rc.ch[3] = ((sbus_buf[5] >> 1 | sbus_buf[6] << 7) & 0x07FF);                     // Channel 4
            rc_ctrl->rc.ch[0] -= RC_CH_VALUE_OFFSET;
            rc_ctrl->rc.ch[1] -= RC_CH_VALUE_OFFSET;
            rc_ctrl->rc.ch[2] -= RC_CH_VALUE_OFFSET;
            rc_ctrl->rc.ch[3] -= RC_CH_VALUE_OFFSET;
            rc_ctrl->rc.ch[4] = ((sbus_buf[6] >> 4 | sbus_buf[7] << 4) & 0x07FF); // Channel 5
            rc_ctrl->rc.ch[4] -= RC_CH_VALUE_OFFSET;
            if (rc_ctrl->rc.ch[4] < 0)
                rc_ctrl->rc.ch[4] = RC_SW_UP;
            else if (rc_ctrl->rc.ch[4] == 0)
                rc_ctrl->rc.ch[4] = RC_SW_MID;
            else if (rc_ctrl->rc.ch[4] > 0)
                rc_ctrl->rc.ch[4] = RC_SW_DOWN;
            rc_ctrl->rc.ch[5] = ((sbus_buf[7] >> 7 | sbus_buf[8] << 1 | sbus_buf[9] << 9) & 0x07FF); // Channel 6
            rc_ctrl->rc.ch[5] -= RC_CH_VALUE_OFFSET;
            if (rc_ctrl->rc.ch[5] < 0)
                rc_ctrl->rc.ch[5] = RC_SW_UP;
            else if (rc_ctrl->rc.ch[5] == 0)
                rc_ctrl->rc.ch[5] = RC_SW_MID;
            else if (rc_ctrl->rc.ch[5] > 0)
                rc_ctrl->rc.ch[5] = RC_SW_DOWN;
            rc_ctrl->rc.ch[6] = ((sbus_buf[9] >> 2 | sbus_buf[10] << 6) & 0x07FF); // Channel 7
            rc_ctrl->rc.ch[6] -= RC_CH_VALUE_OFFSET;
            if (rc_ctrl->rc.ch[6] < 0)
                rc_ctrl->rc.ch[6] = RC_SW_UP;
            else if (rc_ctrl->rc.ch[6] == 0)
                rc_ctrl->rc.ch[6] = RC_SW_MID;
            else if (rc_ctrl->rc.ch[6] > 0)
                rc_ctrl->rc.ch[6] = RC_SW_DOWN;
            rc_ctrl->rc.ch[7] = ((sbus_buf[10] >> 5 | sbus_buf[11] << 3) & 0x07FF); // Channel 8
            rc_ctrl->rc.ch[7] -= RC_CH_VALUE_OFFSET;
            if (rc_ctrl->rc.ch[7] < 0)
                rc_ctrl->rc.ch[7] = RC_SW_UP;
            else if (rc_ctrl->rc.ch[7] == 0)
                rc_ctrl->rc.ch[7] = RC_SW_MID;
            else if (rc_ctrl->rc.ch[7] > 0)
                rc_ctrl->rc.ch[7] = RC_SW_DOWN;

            #ifdef SIM_STUDIO_RC_CH_CHANNEL
            rcCh_to_stm_studio();
            #endif
        }
    }
}

uint8_t rc_is_powerless(void){
    return rcPowerlessFlag;
}
